﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using Microsoft.DirectX;
using Microsoft.DirectX.Direct3D;

namespace Final_Project.Logic.KinectFusion
{
    class CBuildPointCloud
    {
        public DepthImagePoint[] DepthPointCloud;
        public ColorImagePoint[] ColorPointCloud;
        public Vector3[] GlobalPointCloud;
        public Vector3 averageDistance;
        private int m_nColumns;
        private int m_nRows;
        //dunno if that is smart i think we need to have only one static or public or something
        private KinectSensor m_cSensor;


        public CBuildPointCloud(int Columns, int Rows)
        {
            DepthPointCloud = new DepthImagePoint[Columns * Rows];
            ColorPointCloud = new ColorImagePoint[Columns * Rows];
            GlobalPointCloud = new Vector3[Columns * Rows];
            m_nColumns = Columns;
            m_nRows = Rows;
            m_cSensor = KinectSensor.KinectSensors.First();
        }


        private void BuildPointCloudVertex(ref short[] cCurDepthShortData)
        {
            averageDistance = new Vector3(0, 0, 0);
            int k = 0;
            int cx = m_nColumns;
            int cy = m_nRows;
            int distance = 2;
            double scale = 0.001f;

            double fxinv = 1.0 / 600;
            double fyinv = 1.0 / 600;

            //double scale = 0.001;

            for (int i = 0; i < m_nColumns; i++)
            {
                for (int j = 0; j < m_nRows; j++)
                {
                    if (cCurDepthShortData[k] > 0)
                    {
                        int depth = cCurDepthShortData[k] >> 3;
                        GlobalPointCloud[k].X = (float)(scale * ((cx - j) * depth * fxinv));
                        GlobalPointCloud[k].Y = (float)(scale * ((cy - i) * depth * fyinv));
                        GlobalPointCloud[k].Z = (float)(depth * scale) + distance;

                        DepthPointCloud[k].X = j;
                        DepthPointCloud[k].Y = i;
                        DepthPointCloud[k].Depth = depth;

                        averageDistance.X += GlobalPointCloud[k].X;
                        averageDistance.Y += GlobalPointCloud[k].Y;
                        averageDistance.Z += GlobalPointCloud[k].Z;
                    }
                    k++;
                }
            }
        }
        //WILL cause problems because depth stream is not the same size. should change the sensor aswell.
        private void GenerateRGBCloud(ref short[] cCurDepthShortData)
        {
            for (int k = 0; k < m_nColumns * m_nRows; k++)
            {
                if (cCurDepthShortData[k] > 0)
                    ColorPointCloud[k] = m_cSensor.CoordinateMapper.MapDepthPointToColorPoint(DepthImageFormat.Resolution320x240Fps30, DepthPointCloud[k], ColorImageFormat.RgbResolution640x480Fps30);
            }
        }
    }
}
